/*
 * 文件名：A_Star.cpp
 * 功能描述：A* 算法路径规划实现（基于网格地图）
 */

#include "A_Star.h"

// 定义方向：卡尔登方向（上下左右）和全方向（包含对角线）
std::vector<std::pair<int, int>> cardinal_directions = {{0, 1}, {1, 0}, {0, -1}, {-1, 0}};                                // 右、下、左、上
std::vector<std::pair<int, int>> all_directions = {{0, 1}, {1, 0}, {0, -1}, {-1, 0}, {1, 1}, {-1, -1}, {1, -1}, {-1, 1}}; // 包含对角线方向

// 根据较小的10x10网格初始化19x19网格
void initializeGrid(char grid[GRID_SIZE][GRID_SIZE], char grid_10x10[10][10])
{
    // 初始化网格的所有单元格为 '0'（空格）
    for (int i = 0; i < GRID_SIZE; ++i)
    {
        for (int j = 0; j < GRID_SIZE; ++j)
        {
            grid[i][j] = '+';
        }
    }

    // 将10x10网格复制到19x19网格的偶数位置
    for (int i = 0; i < 10; ++i)
    {
        for (int j = 0; j < 10; ++j)
        {
            grid[i * 2][j * 2] = grid_10x10[i][j];
        }
    }

    // 将奇数行和列填充为 '2'（障碍物）
    for (int i = 1; i < GRID_SIZE; i += 2)
    {
        for (int j = 0; j < GRID_SIZE; ++j)
        {
            grid[i][j] = '2'; // 设置奇数行为障碍物
        }
    }
    for (int j = 1; j < GRID_SIZE; j += 2)
    {
        for (int i = 0; i < GRID_SIZE; ++i)
        {
            grid[i][j] = '2'; // 设置奇数列为障碍物
        }
    }
}
// 定义禁止的坐标（不再作为参数传递）
// 完全禁止通过的坐标点
std::vector<std::pair<int, int>> forbidden_coords = {
    // {1, 1},
    // {2, 1},
    // {3, 1},
    // {4, 1},
    // {5, 1},
    // {6, 1},
    // {7, 1},
    // {8, 1},
    // {9, 1},
    // {10, 1},
    // {11, 1},
    // {12, 1},
    // {13, 1},
    // {14, 1},
    // {15, 1},
    // {16, 1},
    // {17, 1},

    // {1, 17},
    // {2, 17},
    // {3, 17},
    // {4, 17},
    // {5, 17},
    // {6, 17},
    // {7, 17},
    // {8, 17},
    // {9, 17},
    // {10, 17},
    // {11, 17},
    // {12, 17},
    // {13, 17},
    // {14, 17},
    // {15, 17},
    // {16, 17},

    // {1, 2},
    // {1, 3},
    // {1, 4},
    // {1, 5},
    // {1, 6},
    // {1, 7},
    // {1, 8},
    // {1, 9},
    // {1, 10},
    // {1, 11},
    // {1, 12},
    // {1, 13},
    // {1, 14},
    // {1, 15},
    // {1, 16},

    // {17, 2},
    // {17, 3},
    // {17, 4},
    // {17, 5},
    // {17, 6},
    // {17, 7},
    // {17, 8},
    // {17, 9},
    // {17, 10},
    // {17, 11},
    // {17, 12},
    // {17, 13},
    // {17, 14},
    // {17, 15},
    // {17, 16},
};

// 低优先级坐标数组 - 这些点可以通过但会有额外的成本，算法会尽量避开
std::vector<std::pair<int, int>> low_priority_coords = {
    // 在这里添加需要避开的坐标点
    {1, 1},
    {2, 1},
    {3, 1},
    {4, 1},
    {5, 1},
    {6, 1},
    {7, 1},
    {8, 1},
    {9, 1},
    {10, 1},
    {11, 1},
    {12, 1},
    {13, 1},
    {14, 1},
    {15, 1},
    {16, 1},
    {17, 1},

    {1, 17},
    {2, 17},
    {3, 17},
    {4, 17},
    {5, 17},
    {6, 17},
    {7, 17},
    {8, 17},
    {9, 17},
    {10, 17},
    {11, 17},
    {12, 17},
    {13, 17},
    {14, 17},
    {15, 17},
    {16, 17},

    {1, 2},
    {1, 3},
    {1, 4},
    {1, 5},
    {1, 6},
    {1, 7},
    {1, 8},
    {1, 9},
    {1, 10},
    {1, 11},
    {1, 12},
    {1, 13},
    {1, 14},
    {1, 15},
    {1, 16},

    {17, 2},
    {17, 3},
    {17, 4},
    {17, 5},
    {17, 6},
    {17, 7},
    {17, 8},
    {17, 9},
    {17, 10},
    {17, 11},
    {17, 12},
    {17, 13},
    {17, 14},
    {17, 15},
    {17, 16},
};

// 检查附近是否有障碍物（检查 '1'）
bool hasObstacleNearby(int x, int y, char grid[GRID_SIZE][GRID_SIZE])
{
    // 遍历卡尔登方向（上下左右）检查附近的障碍物
    for (const auto &dir : cardinal_directions)
    {
        int nx = x + dir.first;
        int ny = y + dir.second;
        if (nx >= 0 && nx < GRID_SIZE && ny >= 0 && ny < GRID_SIZE && grid[nx][ny] == 'w')
        {
            return true; // 如果有障碍物，则返回 true
        }
    }
    return false; // 没有障碍物，返回 false
}

// A* 算法的启发式函数
int heuristic(const std::pair<int, int> &a, const std::pair<int, int> &b)
{
    // 使用曼哈顿距离作为启发式函数（横向和纵向的距离之和）
    return abs(a.first - b.first) + abs(a.second - b.second);
}

// 添加一个低优先级坐标点
void addLowPriorityCoord(int x, int y)
{
    // 检查坐标是否已存在
    if (std::find(low_priority_coords.begin(), low_priority_coords.end(), std::make_pair(x, y)) == low_priority_coords.end())
    {
        low_priority_coords.push_back(std::make_pair(x, y));
    }
}

// 添加多个低优先级坐标点
void addLowPriorityCoords(const std::vector<std::pair<int, int>> &coords)
{
    for (const auto &coord : coords)
    {
        addLowPriorityCoord(coord.first, coord.second);
    }
}

// 清空低优先级坐标点列表
void clearLowPriorityCoords()
{
    low_priority_coords.clear();
}

// A* 算法寻找路径

std::vector<std::pair<int, int>> aStar(char grid[GRID_SIZE][GRID_SIZE], const std::pair<int, int> &start, const std::pair<int, int> &end)
{
    static int parent_x[GRID_SIZE][GRID_SIZE]; // 用于记录父节点的 x 坐标
    static int parent_y[GRID_SIZE][GRID_SIZE]; // 用于记录父节点的 y 坐标
    static bool visited[GRID_SIZE][GRID_SIZE]; // 标记每个节点是否已被访问

    // 初始化父节点和访问标记
    memset(parent_x, -1, sizeof(parent_x));
    memset(parent_y, -1, sizeof(parent_y));
    memset(visited, 0, sizeof(visited));

    // 定义优先队列的比较函数
    auto compare = [](const std::pair<int, std::pair<int, int>> &a, const std::pair<int, std::pair<int, int>> &b)
    {
        return a.first > b.first; // 根据 f 值排序
    };

    // 优先队列，存储节点及其 f 值
    std::priority_queue<std::pair<int, std::pair<int, int>>,
                        std::vector<std::pair<int, std::pair<int, int>>>,
                        decltype(compare)>
        open_list(compare);

    // 将起始点加入 open_list
    open_list.push({heuristic(start, end), start});
    visited[start.first][start.second] = true; // 标记起始点为已访问

    while (!open_list.empty())
    {
        // 取出 f 值最小的节点
        auto current = open_list.top().second;
        int current_cost = open_list.top().first - heuristic(current, end);
        open_list.pop();

        // 如果到达终点，则回溯路径并返回
        if (current == end)
        {
            std::vector<std::pair<int, int>> path;
            std::pair<int, int> node = end;
            while (node != start)
            {
                path.push_back(node);
                int px = parent_x[node.first][node.second];
                int py = parent_y[node.first][node.second];
                node = {px, py};
            }
            path.push_back(start);                  // 将起点添加到路径中
            std::reverse(path.begin(), path.end()); // 将路径反转，使之从起点到终点
            return path;                            // 返回最终路径
        }

        // 根据当前位置是否有障碍物来选择方向（如果附近有障碍物，使用卡尔登方向，否则使用全方向）
        const auto &directions = hasObstacleNearby(current.first, current.second, grid) ? std::vector<std::pair<int, int>>{{0, 1}, {1, 0}, {0, -1}, {-1, 0}} : std::vector<std::pair<int, int>>{{0, 1}, {1, 0}, {0, -1}, {-1, 0}, {1, 1}, {-1, -1}, {1, -1}, {-1, 1}};
        // const auto &directions = cardinal_directions;
        // 遍历所有方向
        for (const auto &dir : directions)
        {
            int nx = current.first + dir.first;
            int ny = current.second + dir.second;
            // 检查新的位置是否合法，且没有障碍物且未被访问过，并且不在禁止列
            if (nx >= 0 && nx < GRID_SIZE && ny >= 0 && ny < GRID_SIZE && grid[nx][ny] != 'w' && !visited[nx][ny] && std::find(forbidden_coords.begin(), forbidden_coords.end(), std::make_pair(nx, ny)) == forbidden_coords.end())
            {
                visited[nx][ny] = true;            // 标记新位置为已访问
                parent_x[nx][ny] = current.first;  // 记录父节点的 x 坐标
                parent_y[nx][ny] = current.second; // 记录父节点的 y 坐标

                // 计算新的移动成本，如果是低优先级区域，增加额外成本
                int new_cost = current_cost + 1;

                // 检查当前点是否在低优先级坐标列表中
                if (std::find(low_priority_coords.begin(), low_priority_coords.end(), std::make_pair(nx, ny)) != low_priority_coords.end())
                {
                    // 该点是低优先级点，增加额外成本
                    new_cost += 5; // 增加的成本值可以调整，值越大越倾向于避开该点
                }

                open_list.push({new_cost + heuristic({nx, ny}, end), {nx, ny}}); // 将新节点加入优先队列
            }
        }
    }
    return {}; // 如果无法到达终点，返回空路径
}